{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Particle filter\n",
    "===============\n",
    "\n",
    "A basic particle filter tracking algorithm, using a uniformly\n",
    "distributed step as motion model, and the initial target colour as\n",
    "determinant feature for the weighting function. This requires an\n",
    "approximately uniformly coloured object, which moves at a speed no\n",
    "larger than stepsize per frame.\n",
    "\n",
    "This implementation assumes that the video stream is a sequence of numpy\n",
    "arrays, an iterator pointing to such a sequence or a generator\n",
    "generating one. The particle filter itself is a generator to allow for\n",
    "operating on real-time video streams."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "#!python\n",
    "from numpy import *\n",
    "from numpy.random import *\n",
    "\n",
    "\n",
    "def resample(weights):\n",
    "  n = len(weights)\n",
    "  indices = []\n",
    "  C = [0.] + [sum(weights[:i+1]) for i in range(n)]\n",
    "  u0, j = random(), 0\n",
    "  for u in [(u0+i)/n for i in range(n)]:\n",
    "    while u > C[j]:\n",
    "      j+=1\n",
    "    indices.append(j-1)\n",
    "  return indices\n",
    "\n",
    "\n",
    "def particlefilter(sequence, pos, stepsize, n):\n",
    "  seq = iter(sequence)\n",
    "  x = ones((n, 2), int) * pos                   # Initial position\n",
    "  f0 = seq.next()[tuple(pos)] * ones(n)         # Target colour model\n",
    "  yield pos, x, ones(n)/n                       # Return expected position, particles and weights\n",
    "  for im in seq:\n",
    "    np.add(x, uniform(-stepsize, stepsize, x.shape), out=x, casting=\"unsafe\")  # Particle motion model: uniform step\n",
    "    x  = x.clip(zeros(2), array(im.shape)-1).astype(int) # Clip out-of-bounds particles\n",
    "    f  = im[tuple(x.T)]                         # Measure particle colours\n",
    "    w  = 1./(1. + (f0-f)**2)                    # Weight~ inverse quadratic colour distance\n",
    "    w /= sum(w)                                 # Normalize w\n",
    "    yield sum(x.T*w, axis=1), x, w              # Return expected position, particles and weights\n",
    "    if 1./sum(w**2) < n/2.:                     # If particle cloud degenerate:\n",
    "      x  = x[resample(w),:]                     # Resample particles according to weights"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The following code shows the tracker operating on a test sequence\n",
    "featuring a moving square against a uniform background."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAU0AAAD8CAYAAADzEfagAAAABHNCSVQICAgIfAhkiAAAAAlwSFlz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\nU5Iq/D/cl5yJGQxruAAAAABJRU5ErkJggg==\n",
      "text/plain": [
       "<matplotlib.figure.Figure at 0x10f5dbbd0>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/plain": [
       "None"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "#!python\n",
    "if __name__ == \"__main__\":\n",
    "  from pylab import *\n",
    "  from itertools import izip\n",
    "  import time\n",
    "  from IPython import display\n",
    "\n",
    "  ion()\n",
    "  seq = [ im for im in zeros((20,240,320), int)]      # Create an image sequence of 20 frames long\n",
    "  x0 = array([120, 160])                              # Add a square with starting position x0 moving along trajectory xs\n",
    "  xs = vstack((arange(20)*3, arange(20)*2)).T + x0\n",
    "  for t, x in enumerate(xs):\n",
    "    xslice = slice(x[0]-8, x[0]+8)\n",
    "    yslice = slice(x[1]-8, x[1]+8)\n",
    "    seq[t][xslice, yslice] = 255\n",
    "\n",
    "  for im, p in izip(seq, particlefilter(seq, x0, 8, 100)): # Track the square through the sequence\n",
    "    pos, xs, ws = p\n",
    "    position_overlay = zeros_like(im)\n",
    "    position_overlay[np.array(pos).astype(int)] = 1\n",
    "    particle_overlay = zeros_like(im)\n",
    "    particle_overlay[tuple(xs.T)] = 1\n",
    "    draw()\n",
    "    time.sleep(0.3)\n",
    "    clf()                                           # Causes flickering, but without the spy plots aren't overwritten\n",
    "    imshow(im,cmap=cm.gray)                         # Plot the image\n",
    "    spy(position_overlay, marker='.', color='b')    # Plot the expected position\n",
    "    spy(particle_overlay, marker=',', color='r')    # Plot the particles\n",
    "    display.clear_output(wait=True)\n",
    "    display.display(show())"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 2",
   "language": "python",
   "name": "python2"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 2
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython2",
   "version": "2.7.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}
